classdef SdInertNavIntTest < matlab.unittest.TestCase
    % 验证SdInertNavInt函数
    properties(TestParameter)
        g = {9.7934752114984}
        CfgPar = mat2cell(cell2struct( ...
            {false, NavCoordType.WAN_AZM, true, true, 0.02, [0 0 0];                         % 1. 基本配置 四元数/WANAZM/高精度位置更新/无器件误差
            true, NavCoordType.WAN_AZM, true, true, 0.02, [0 0 0];                           % 2. 配置1 DCM/WANAZM/高精度位置更新/无器件误差
            false, NavCoordType.GEO, true, true, 0.02, [0 0 0];                              % 3. 配置2 四元数/GEO/高精度位置更新/无器件误差
            false, NavCoordType.FREE_AZM, true, true, 0.02, [0 0 0];                         % 4. 配置3 四元数/FREEAZM/高精度位置更新/无器件误差
            false, NavCoordType.WAN_AZM, false, true, 0.02, [0 0 0];                         % 5. 配置4 四元数/WANAZM/低精度位置更新/无器件误差
            false, NavCoordType.WAN_AZM, true, true, 0.02, [2e-6, 0.00040308, 0.03];         % 6. 配置5 四元数/WANAZM/高精度位置更新/考虑垂直通道阻尼/无器件误差
            false, NavCoordType.WAN_AZM, true, false, 0.02, [0 0 0];                         % 7. 配置6 四元数/WANAZM/高精度位置更新/不考虑垂直通道阻尼/有器件误差
            false, NavCoordType.WAN_AZM, true, false, 0.02, [2e-6, 0.00040308, 0.03];}', ... % 8. 配置7 四元数/WANAZM/高精度位置更新/考虑垂直通道阻尼/有器件误差
            {'useDCMInAttCalc', 'navCoordType', 'useHiResPosNSCal', 'IMUErrFree', 'samplePeriod', 'C'}), ones(8, 1))
        Cst = {load('earthconstants_WGS84.mat')}
    end
    
    methods(Test)
        function vldSdInertNavIntTest(testCase, g, CfgPar, Cst)
            %% 生成轨迹
            if ~exist('SdInertNavIntTest.mat', 'file')
                deg2Rad = pi / 180;
                KT = KinemTraj(CfgPar.samplePeriod, g, Cst);%创建KinemTraj类对象KT
                KT.InitCon.pos = [30 * deg2Rad, 110 * deg2Rad, 0]';
                KT.Cfg.IMU.enableNoise = true;
                KT.Cfg.IMU.ignore2OdErr = false;
                ATP = AccTriadPars;
                ATP.dfBiasS = [33 33 33]' * g * 1e-6;
                KT.Par.IMU.acc = ATP;
                KT = KT.genTrajGenIn_veh('gentgi_navtest_regular', {});
                out_trajGen = KT.runTrajGen;
                save('SdInertNavIntTest.mat', 'out_trajGen');
            else
                load('SdInertNavIntTest.mat', 'out_trajGen');
            end
            
            %% 惯性导航
            if CfgPar.IMUErrFree % 输入量
                in.accOut = out_trajGen.IMU.accOutErrFree;
                in.gyroOut = out_trajGen.IMU.gyroOutErrFree;
            else
                in.accOut = out_trajGen.IMU.accOut;
                in.gyroOut = out_trajGen.IMU.gyroOut;
            end
            
            SINI = SdInertNavInt('Cfg_UseDCMInAttCalc', CfgPar.useDCMInAttCalc, 'Cfg_UseHiResPosNSCal', CfgPar.useHiResPosNSCal, ...
                'Cfg_NavCoordType', CfgPar.navCoordType, 'Par_Tl', CfgPar.samplePeriod, 'Par_C', CfgPar.C, ...
                'InitCon_LLA_n', out_trajGen.pos(1, :, 1)', 'InitCon_LLA_n1', out_trajGen.pos(1, :, 1)', ...
                'InitCon_vN_m', out_trajGen.vG(1, :, 1)', 'InitCon_vN_m1', out_trajGen.vG(1, :, 1)', 'InitCon_vN_n1', out_trajGen.vG(1, :, 1)', ...
                'InitCon_CBL', [0 1 0; 1 0 0; 0 0 -1]*out_trajGen.CBG(:, :, 1), 'InitCon_g', g, ...
                'InitCon_specVelInc_lx', in.accOut(1, :)', 'InitCon_angleInc_lx', in.gyroOut(1, :)');
            [out_SINI, ~] = SINI.stepThrough(in.accOut, in.gyroOut, [], out_trajGen.pos(2:end, 3));
            
            %% 导航参数测试
            out_SINI.t = out_SINI.cycCnt*SINI.Par_Tl;
            out_SINI.pos = [out_SINI.L, out_SINI.lambda, out_SINI.h];
            out_trajGen.vG = out_trajGen.vG(:, :, 3);
            out_trajGen.pos = out_trajGen.pos(:, :, 3);
            dp = comparenavparam({out_SINI}, out_trajGen, Cst.RE, [], {'导航'}, '轨迹');
            disp('最大位置误差（m）');
            disp(max(abs(dp.dPos{1}), [], 1));
            disp('最大速度误差（m/s）');
            disp(max(abs(dp.dvG{1}), [], 1));
            disp('最大姿态误差（″）');
            disp(max(abs(dp.phi{1}), [], 1));
            disp('最大正交误差（″）');
            disp(max(abs(dp.dOrth{1}), [], 1));
            disp('最大规范误差（ppm）');
            disp(max(abs(dp.dNorm{1}), [], 1));
            if CfgPar.IMUErrFree
                testCase.verifyLessThanOrEqual(max(abs(dp.dPos{1}), [], 1), 0.0060);
                testCase.verifyLessThanOrEqual(max(abs(dp.dvG{1}), [], 1), 1e-5);
                testCase.verifyLessThanOrEqual(max(abs(dp.phi{1}), [], 1), 1e-3);
                testCase.verifyLessThanOrEqual(max(abs(dp.dOrth{1}), [], 1), 1e-9);
                testCase.verifyLessThanOrEqual(max(abs(dp.dNorm{1}), [], 1), 1e-8);
            end
        end
    end
end